#include "AutoPos1.h"
#include "FireDisc.h"
#include "PulseBreach.h"
#include "ChangeConveyorStatus.h"
#include "SetShooterAngle.h"

AutoPos1::AutoPos1() {
	// Add Commands here:
	// e.g. AddSequential(new Command1());
	//      AddSequential(new Command2());
	// these will run in order.
	// To run multiple commands at the same time,
	// use AddParallel()
	// e.g. AddParallel(new Command1());
	//      AddSequential(new Command2());
	// Command1 and Command2 will run in parallel.
	// A command group will require all of the subsystems that each member
	// would require.
	// e.g. if Command1 requires chassis, and Command2 requires arm,
	// a CommandGroup containing them would require both the chassis and the
	// arm.
	//TODO: Salmon this
	//Starting: 18
	//Shaved time:
	//	0.25
	//	0.5
	//	0.5
	//	
	//	
	//	
	
	float AUTOANGLE = 23.5;
	AddSequential(new WaitCommand(4.0));
	AddSequential(new FireDisc());
	AddSequential(new WaitCommand(0.5));
	AddSequential(new SetShooterAngle(4.0));
	AddSequential(new WaitCommand(1.0));
	AddSequential(new PulseBreach());
	AddSequential(new WaitCommand(1.75));
	AddSequential(new SetShooterAngle(AUTOANGLE));
	AddSequential(new ChangeConveyorStatus(DiscHandling::kConveyorUp));
	AddSequential(new WaitCommand(2.0));
	AddSequential(new FireDisc());
	AddSequential(new WaitCommand(0.5));
	AddSequential(new SetShooterAngle(4.0));
	AddSequential(new WaitCommand(1.0));
	AddSequential(new PulseBreach());
	AddSequential(new WaitCommand(2.0));
	AddSequential(new SetShooterAngle(AUTOANGLE));
	AddSequential(new WaitCommand(1.0));
	AddSequential(new FireDisc());
}
